#include "brush_model.h"
namespace auto_ros
{
namespace control
{
BrushModel::BrushModel(auto_ros::common_tool::double_map parm) : TireModelBase(parm)
{
	//map_iter auto_ros::common_tool::find_in_map<double>(std::string which_var, auto_ros::common_tool::double_map & in_map)
	auto_ros::common_tool::find_in_map<double>("mu", parm_);
	auto_ros::common_tool::find_in_map<double>("c_alpha", parm_);
}

double BrushModel::calc_fy(auto_ros::common_tool::double_map &input)
{
	double alpha_rad = auto_ros::common_tool::find_in_map<double>("alpha_rad", input);
	double fz = auto_ros::common_tool::find_in_map<double>("fz", input);
	double tan_alpha = std::tan(alpha_rad);
	double c_alpha = parm_["c_alpha"];
	double mu = parm_["mu"];
	double bool_var = std::atan(3.0 * mu * fz / c_alpha);
	if (std::abs(alpha_rad) < bool_var)
	{
		return -c_alpha * tan_alpha +
			   std::pow(c_alpha, 2.0) / 3 / mu / fz * std::abs(tan_alpha) * tan_alpha -
			   std::pow(c_alpha, 3.0) / 27 / std::pow(mu, 2.0) / fz / fz * std::pow(tan_alpha, 3.0);
	}
	else if (alpha_rad > 0.0)
	{
		return -mu * fz;
	}
	else
	{
		return mu * fz;
	}
}
double BrushModel::calc_dyn_c_alpha(auto_ros::common_tool::double_map &input)
{
	double alpha_rad = auto_ros::common_tool::find_in_map<double>("alpha_rad", input);
	double fz = auto_ros::common_tool::find_in_map<double>("fz", input);
	double tan_alpha = std::tan(alpha_rad);
	double c_alpha = parm_["c_alpha"];
	double mu = parm_["mu"];
	double bool_var = std::atan(3.0 * mu * fz / c_alpha);
	double temp_var;
	double deri_tan_alpha = 1.0 / std::pow(std::cos(alpha_rad), 2.0);
	if (std::abs(alpha_rad) < bool_var)
	{
		temp_var = -c_alpha + 2.0 * c_alpha * c_alpha / 3.0 / mu / fz * std::abs(tan_alpha) -
				   std::pow(c_alpha, 3.0) / 9.0 / std::pow(mu, 2.0) / fz / fz * std::pow(tan_alpha, 2.0);
	}

	else
	{
		temp_var = 0.0;
	}
	return temp_var * deri_tan_alpha;
}
double BrushModel::calc_static_c_alpha(auto_ros::common_tool::double_map &input)
{
	double alpha_rad = auto_ros::common_tool::find_in_map<double>("alpha_rad", input);
	if (std::abs(alpha_rad) < 0.01)
	{
		return calc_dyn_c_alpha(input);
	}
	else
	{
		double fy = calc_fy(input);
		return fy / alpha_rad;
	}
}
} // namespace control
} // namespace auto_ros